#!/usr/bin/python3

import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, PoseStamped
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import LaserScan
from gazebo_msgs.msg import ModelStates
import copy
# from p903_simulation.srv import ModulatingToZhujianpanCmd, ModulatingToZhujianpanCmdResponse
from tf2_geometry_msgs import tf2_geometry_msgs

class p903_system_service():
    def __init__(self,nodename):
        rospy.init_node(nodename)

        # self.zhujianpanLink = "zhongduan/zuobiaoxi_meas"
        # self.modulatingLink = "fine_machining/modulating_Link"
        # self.bottomLink = "fine_machining/bottom_Link"
        # self.platformLink = "fine_machining/platform_Link"

        self.tfBroadcasterDyamic =tf2_ros.TransformBroadcaster()
        self.tfBroadcaster=tf2_ros.StaticTransformBroadcaster()
        self.staticTransformList = []

        self.tfBuffer = tf2_ros.Buffer()
        self.tfListener = tf2_ros.TransformListener(self.tfBuffer)

        if(rospy.has_param("key_components")):
            self.component_names = rospy.get_param("key_components")
            self.key_components = {}
            for i in self.component_names:
                self.key_components[i] = rospy.get_param(i+"/born")
        else:
            self.component_names = {}

        # self.establish_key_components(self.key_components)

        if(rospy.has_param("moveable_components")):
            self.moveable_components = rospy.get_param("moveable_components")
        else:
            self.moveable_components = {}

        # self.holdingTopic = rospy.Subscriber("holding_state",Float64MultiArray,self.cb_holdingState)
        # rospy.Service("get_zhujian_pan_pose",GetZhujianPanPose, self.get_zhujian_pan_pose)
        # rospy.Service("align_modulatingLink_zhujianpan",ModulatingToZhujianpanCmd, self.modulatingTozhujianpan_cb)
        # self.stewartPtpMove = rospy.Publisher('fine_machining/stewart_base_command', Float64MultiArray, queue_size=10)

        self.gazeboModelStates = rospy.Subscriber("/gazebo/model_states",ModelStates,self.cb_modelStates,queue_size = 2)
        self.gazeboModelStatesCntr = 0

    def cb_modelStates(self,msg):
        if(self.gazeboModelStatesCntr < 10):
            self.gazeboModelStatesCntr = self.gazeboModelStatesCntr +1
            return
        else:
            self.gazeboModelStatesCntr = 0

        trans_list = []

        curr_time = rospy.Time.now()
        name_rev = {msg.name[i]:i for i in range(len(msg.name))}

        for i in self.component_names:
            trans = TransformStamped()
            if(i in msg.name):
                index = name_rev[i]

                trans.header.frame_id = "world"
                trans.header.stamp = curr_time
                trans.child_frame_id = msg.name[index] + "/base_link"

                trans.transform.translation.x = msg.pose[index].position.x
                trans.transform.translation.y = msg.pose[index].position.y
                trans.transform.translation.z = msg.pose[index].position.z

                trans.transform.rotation.x = msg.pose[index].orientation.x
                trans.transform.rotation.y = msg.pose[index].orientation.y
                trans.transform.rotation.z = msg.pose[index].orientation.z
                trans.transform.rotation.w = msg.pose[index].orientation.w
                trans_list.append(trans)
            else:
                trans.header.frame_id = "disappear"
                trans.header.stamp = curr_time
                trans.child_frame_id = i + "/base_link"

                trans.transform.translation.x = 0.0
                trans.transform.translation.y = 0.0
                trans.transform.translation.z = 0.0

                trans.transform.rotation.x = 0.0
                trans.transform.rotation.y = 0.0
                trans.transform.rotation.z = 0.0
                trans.transform.rotation.w = 1.0
                trans_list.append(trans)

        for i in self.moveable_components:
            trans = TransformStamped()
            if(i in msg.name):
                index = name_rev[i]

                trans.header.frame_id = "world"
                trans.header.stamp = curr_time
                trans.child_frame_id = msg.name[index] + "/base_link"

                trans.transform.translation.x = msg.pose[index].position.x
                trans.transform.translation.y = msg.pose[index].position.y
                trans.transform.translation.z = msg.pose[index].position.z

                trans.transform.rotation.x = msg.pose[index].orientation.x
                trans.transform.rotation.y = msg.pose[index].orientation.y
                trans.transform.rotation.z = msg.pose[index].orientation.z
                trans.transform.rotation.w = msg.pose[index].orientation.w
                trans_list.append(trans)
            else:
                trans.header.frame_id = "disappear"
                trans.header.stamp = curr_time
                trans.child_frame_id = i + "/base_link"

                trans.transform.translation.x = 0.0
                trans.transform.translation.y = 0.0
                trans.transform.translation.z = 0.0

                trans.transform.rotation.x = 0.0
                trans.transform.rotation.y = 0.0
                trans.transform.rotation.z = 0.0
                trans.transform.rotation.w = 1.0
                trans_list.append(trans)

        # for i in range(len(msg.name)):
        #     if(msg.name[i] in self.component_names):
        #         trans = TransformStamped()
        #         trans.header.frame_id = "world"
        #         trans.header.stamp = curr_time
        #         trans.child_frame_id = msg.name[i] + "/base_link"

        #         trans.transform.translation.x = msg.pose[i].position.x
        #         trans.transform.translation.y = msg.pose[i].position.y
        #         trans.transform.translation.z = msg.pose[i].position.z

        #         trans.transform.rotation.x = msg.pose[i].orientation.x
        #         trans.transform.rotation.y = msg.pose[i].orientation.y
        #         trans.transform.rotation.z = msg.pose[i].orientation.z
        #         trans.transform.rotation.w = msg.pose[i].orientation.w
        #         trans_list.append(trans)
        #         # print(trans.child_frame_id)

        #     if(msg.name[i] in self.moveable_components):
        #         trans = TransformStamped()
        #         trans.header.frame_id = "world"
        #         trans.header.stamp = curr_time
        #         trans.child_frame_id = msg.name[i] + "/base_link"

        #         trans.transform.translation.x = msg.pose[i].position.x
        #         trans.transform.translation.y = msg.pose[i].position.y
        #         trans.transform.translation.z = msg.pose[i].position.z

        #         trans.transform.rotation.x = msg.pose[i].orientation.x
        #         trans.transform.rotation.y = msg.pose[i].orientation.y
        #         trans.transform.rotation.z = msg.pose[i].orientation.z
        #         trans.transform.rotation.w = msg.pose[i].orientation.w
        #         trans_list.append(trans)
        #         # print(trans.child_frame_id)

        # rospy.sleep(rospy.Duration(0.05))
        self.tfBroadcasterDyamic.sendTransform(trans_list)

    def establish_key_components(self, key_components):

        for i in key_components.keys():
            trans = TransformStamped()

            trans.header.frame_id = "world" 
            trans.header.stamp = rospy.Time.now()
            trans.child_frame_id = i + "/base_link"
            #rospy.loginfo(trans.child_frame_id)

            trans.transform.translation.x = key_components[i][0]
            trans.transform.translation.y = key_components[i][1]
            trans.transform.translation.z = key_components[i][2]

            rotation = tf.transformations.quaternion_from_euler(key_components[i][3], key_components[i][4], key_components[i][5])
            trans.transform.rotation.x = rotation[0]
            trans.transform.rotation.y = rotation[1]
            trans.transform.rotation.z = rotation[2]
            trans.transform.rotation.w = rotation[3]

            self.appendTransform(trans)

        self.sendStaticTransforms()

    def appendTransform(self,trans):
        for i in range(len(self.staticTransformList)):
            if(self.staticTransformList[i].header.frame_id==trans.header.frame_id and 
                self.staticTransformList[i].child_frame_id == trans.child_frame_id):
                print("already exist",self.staticTransformList[i])
                self.staticTransformList[i]=trans
                print("already exist",self.staticTransformList[i])
                return 
        self.staticTransformList.append(trans)
        return 

    def sendStaticTransforms(self):
        if(len(self.staticTransformList)>0):
            self.tfBroadcaster.sendTransform(self.staticTransformList)
        return

    def run(self):
        '''
        rate = rospy.Rate(50)
        trans = TransformStamped()
        while not rospy.is_shutdown():
            if(self.msgNames is None or self.msgPose is None):
                rate.sleep()
                continue

            for i in range(len(self.msgNames)):
                if(self.msgNames[i] in self.baseLinks.keys()):
                    #print(msg.name[i])
                    #print(msg.pose[i])
                    trans.header.frame_id = "world"
                    trans.child_frame_id = self.baseLinks[self.msgNames[i]]

                    trans.header.stamp = rospy.Time.now()

                    trans.transform.translation.x = self.msgPose[i].position.x
                    trans.transform.translation.y = self.msgPose[i].position.y
                    trans.transform.translation.z = self.msgPose[i].position.z

                    trans.transform.rotation.x = self.msgPose[i].orientation.x
                    trans.transform.rotation.y = self.msgPose[i].orientation.y
                    trans.transform.rotation.z = self.msgPose[i].orientation.z
                    trans.transform.rotation.w = self.msgPose[i].orientation.w

                    self.tfBroadcaster.sendTransform(trans)
            rate.sleep()
        '''
        rospy.spin()

if __name__ == '__main__':
    service = p903_system_service('p903_system_service')
    service.run()
